package engine;

import java.util.LinkedList;
import java.util.List;
import java.util.ArrayList;

import quadTree.QuadNode;
import quadTree.BoundingBox.IntersectRes;



public class Kushali {
	
	/*
	 * @ public invariant time>=0;
	 * @ public invariant funcs !=null;
	 */
	private static int statIndex = 0;
	private /*@ spec_public @*/ boolean paused;
	private /*@ spec_public @*/ boolean detectCollisions;
	private /*@ spec_public @*/ boolean writeToLog;
	private /*@ spec_public @*/ float timeScale;
	private /*@ spec_public @*/ Scene scene;
	private /*@ spec_public @*/ Logfile log;
	private /*@ spec_public @*/ SceneFile sf;
	private /*@ spec_public @*/ float time;
	public static PhysicsFunctions funcs;
	private /*@ spec_public @*/ List<Collision> collisions;
	private List<Integer> freeIndices;
	/**
	 * Kushali Constructor
	 */
	/*
	 * @ensures scene != null;
	 * @ensures log != null;
	 * @ensures collisions != null;
	 * @ensures time >= 0;
	 * @ensures funcs != null;
	 */
	public Kushali() {
		paused = false;
		detectCollisions = true;
		writeToLog = false;
		timeScale = 1f;
		scene = new Scene(1000f);
		log = new Logfile(10);
		time = 0f;
		funcs = new PhysicsFunctions();
		collisions = new ArrayList<Collision>();
		sf = new SceneFile();
		freeIndices = new ArrayList<Integer>();
	}
	
	/**
	 * Kushali Constructor that will load in a user specified scene.
	 */
	/*
	 * @ensures scene != null;
	 * @ensures log != null;
	 * @ensures collisions != null;
	 * @ensures time >= 0;
	 * @ensures funcs != null;
	 */
	public Kushali(String filename) {
		paused = false;
		detectCollisions = true;
		writeToLog = false;
		timeScale = 1f;
		this.loadScene(filename);
		log = new Logfile(500);
		time = 0f;
		funcs = new PhysicsFunctions();
		collisions = new ArrayList<Collision>();
		sf = new SceneFile();
		freeIndices = new ArrayList<Integer>();
	}
	/**
	 * update updates the system by the passed number of steps.  The passed number of steps is then modified by the timescale to compute the actual 
	 * amount of change
	 * @param t the number of steps to update the system by
	 */
	/*
	 *@ requires paused == false;
	 *@ ensures time >= \old(time);
	 *@ returns \nothing; 
	 */
	public void update(float t)
	{
		
		if(t<0){System.out.println("Invalid input, cannot move backwards in time");return;}
		if(!paused) {
			
			collisions.clear();
			//System.out.println("Updated@: " + t + "\n");
			
			if(detectCollisions) {
				detectCollisions();
				resolveCollisions(t);
			}
			float time = t * timeScale;
			log.addToBuffer(scene.update(time, writeToLog));
			relocateObjects();
			this.time += time;
		}
		
	}
	/**
	 * createObject creates a PhysicsObject for use by the system
	 * @param points the verticies of the new object
	 * @param mass the mass of the new object
	 * @param position 
	 * @param velocity the velocity of the new object
	 * @return a reference to the new PhysicsObject
	 */
	/*
	 * @ requires points.size()>0;
	 * @ requires this!=null;
	 * @ requires mass>0;
	 * @ensures scene.objects.get(index)!=\old scene.objects.get(index); 
	 * @returns scene.objects.get(index);
	 */
	public PhysicsObject createObject(List<Vertex> points, List<Edge> edges, float mass, Vector velo)
	{
		if(mass<=0){System.out.println("Invalid input, cannot have negative or zero mass");return null;}
		if(points ==null){System.out.println("Invalid input, cannot have null list of points");return null;}
		if(edges ==null){System.out.println("Invalid input, cannot have null list of edges");return null;}
		if(velo ==null){System.out.println("Invalid input, cannot have null velocity");return null;}
		
		int index = 0;
		if(freeIndices.size() > 0) {
			index = freeIndices.remove(0); 
		}
		else {
			index = statIndex++;
		}
		PhysicsObject po = new PhysicsObject(index, points, mass, edges, velo);
		scene.addObject(po);
		return po;
		
	}
	
	/**
	 * insertPhysicsObject generates an index for a user's custom object and inserts it
	 * @param po the custom physics object
	 * @return a reference to the new PhysicsObject
	 */
	/*
	 * @ requires po != null;
	 * @ requires this!=null;
	 * @ensures scene.objects.get(index)!=\old scene.objects.get(index); 
	 * @returns scene.objects.get(index);
	 */
	public PhysicsObject insertPhysicsObject(PhysicsObject po) {
		if(po==null){System.out.println("Invalid Input, cannot insert null PhysicsObject");return null;}
		int index = 0;
		if(freeIndices.size() > 0) {
			index = freeIndices.remove(0); 
		}
		else {
			index = ++statIndex;
		}
		po.setIndex(index);
		scene.addObject(po);
		return po;
	}
	/**
	 * Triangle creates an equilateral triangle in the system
	 * @param position the position of the center of the triangle
	 * @param mass the mass of the triangle
	 * @param length the length of the edges
	 * @param velocity the velocity of the triangle
	 * @return a reference to the triangle
	 */
	/*
	 * @requires position!=null;
	 * @requires mass>0;
	 * @requires length>0;
	 * @requires velocity!=null;
	 * @returns new PhysicsObject;
	 */
	public PhysicsObject Triangle(Vector position, float mass, float length, Vector velocity)
	{
		if(mass<=0){System.out.println("Invalid input, cannot have negative or zero mass");return null;}
		if(length<=0){System.out.println("Invalid input, cannot have negative or zero length");return null;}
		if(position ==null){System.out.println("Invalid input, cannot have null position");return null;}
		if(velocity ==null){System.out.println("Invalid input, cannot have null velocity");return null;}
		
		List<Vertex> verticies = new LinkedList<Vertex>();
		float centerX = position.getX();
		float centerY = position.getY();
		
		float distance = (float) (length*Math.sin(0.523598776)/Math.sin(2.0943951));
		
		float deltaY = (float) (distance*Math.sin(0.523598776));
		float deltaX = (float) (distance*Math.sin( 0.866025404));
		Vertex v1 = new Vertex(new Vector(centerX, centerY+distance));
		Vertex v2 = new Vertex(new Vector(centerX-deltaX, centerY-deltaY));
		Vertex v3 = new Vertex(new Vector(centerX+deltaX, centerY-deltaY));
		verticies.add(v1);
		verticies.add(v2);
		verticies.add(v3);
		
		List<Edge> edges = new LinkedList<Edge>();
		Edge e1 = new Edge(v1,v2,1);
		Edge e2 = new Edge(v2,v3,1);
		Edge e3 = new Edge(v3,v1,1);
		edges.add(e1);
		edges.add(e2);
		edges.add(e3);
		
		return createObject(verticies, edges, mass, velocity);
	}
	/**
	 * Square creates a square in the system
	 * @param position the position of the center of the square
	 * @param mass the mass of the square
	 * @param length the length of the edges
	 * @param velocity the velocity of the square
	 * @return a reference to the square
	 */
	/*
	 * @requires position!=null;
	 * @requires mass>0;
	 * @requires length>0;
	 * @requires velocity!=null;
	 * @returns new PhysicsObject;
	 */
	public PhysicsObject Square(Vector position, float mass, float length, Vector velocity)
	{
		if(mass<=0){System.out.println("Invalid input, cannot have negative or zero mass");return null;}
		if(length<=0){System.out.println("Invalid input, cannot have negative or zero length");return null;}
		if(position ==null){System.out.println("Invalid input, cannot have null position");return null;}
		if(velocity ==null){System.out.println("Invalid input, cannot have null velocity");return null;}
		
		List<Vertex> verticies = new LinkedList<Vertex>();
		float centerX = position.getX();
		float centerY = position.getY();
		Vertex v1 = new Vertex(new Vector(centerX-(length/2),centerY-(length/2)));
		Vertex v2 = new Vertex(new Vector(centerX+(length/2),centerY-(length/2)));
		Vertex v3 = new Vertex(new Vector(centerX+(length/2),centerY+(length/2)));
		Vertex v4 = new Vertex(new Vector(centerX-(length/2),centerY+(length/2)));
		verticies.add(v1);
		verticies.add(v2);
		verticies.add(v3);
		verticies.add(v4);
		
		List<Edge> edges = new LinkedList<Edge>();
		
		Edge e1 = new Edge(v1,v2,1);
		Edge e2 = new Edge(v2,v3,1);
		Edge e3 = new Edge(v3,v4,1);
		Edge e4 = new Edge(v4,v1,1);
		
		edges.add(e1);
		edges.add(e2);
		edges.add(e3);
		edges.add(e4);
		
		return createObject(verticies, edges, mass, velocity);
	}
	/**
	 * Rectangle creates a rectangle in the system
	 * @param position the position of the center of the rectangle
	 * @param mass the mass of the rectangle
	 * @param length the length of two of the edges
	 * @param width the length of the other two edges
	 * @param velocity the velocity of the rectangle
	 * @return a reference to the rectangle
	 */
	/*
	 * @requires position!=null;
	 * @requires mass>0;
	 * @requires length>0;
	 * @requires width>0;e
	 * @requires velocity!=null;
	 * @returns new PhysicsObject;
	 */
	public PhysicsObject Rectangle(Vector position, float mass, float length, float width, Vector velocity)
	{
		if(mass<=0){System.out.println("Invalid input, cannot have negative or zero mass");return null;}
		if(length<=0){System.out.println("Invalid input, cannot have negative or zero length");return null;}
		if(width<=0){System.out.println("Invalid input, cannot have negative or zero width");return null;}
		if(position ==null){System.out.println("Invalid input, cannot have null position");return null;}
		if(velocity ==null){System.out.println("Invalid input, cannot have null velocity");return null;}
		
		List<Vertex> verticies = new LinkedList<Vertex>();
		float centerX = position.getX();
		float centerY = position.getY();
		Vertex v1 = new Vertex(new Vector(centerX-(length/2),centerY-(width/2)));
		Vertex v2 = new Vertex(new Vector(centerX+(length/2),centerY-(width/2)));
		Vertex v3 = new Vertex(new Vector(centerX+(length/2),centerY+(width/2)));
		Vertex v4 = new Vertex(new Vector(centerX-(length/2),centerY+(width/2)));
		verticies.add(v1);
		verticies.add(v2);
		verticies.add(v3);
		verticies.add(v4);
		
		List<Edge> edges = new LinkedList<Edge>();
		
		Edge e1 = new Edge(v1,v2,1);
		Edge e2 = new Edge(v2,v3,1);
		Edge e3 = new Edge(v3,v4,1);
		Edge e4 = new Edge(v4,v1,1);
		
		edges.add(e1);
		edges.add(e2);
		edges.add(e3);
		edges.add(e4);
		
		return createObject(verticies, edges, mass, velocity);
	}
	
	/**
	 * createForce creates a force object for use by the system
	 * @param pos the Position of the force
	 * @param dir the direction of the force
	 * @param range the area the force effects 
	 * @return a reference to the new force
	 */
	/*
	 * @ requires pos != null;
	 * @ requires dir != null;
	 * @ requires range > 0;
	 * @ensures f != null;
	 */
	public Force createForce(Vector pos, Vector dir, float range) {
		if(range <= 0) {
			System.err.println("Range of a force must be positive!");
			return null;
		}
		Force f = new Force(pos, dir, range);
		scene.addForce(f);
		return f;
	}
	
	
	/**
	 * manipulateTime toggles the paused boolean if passed value is 0, modifies the timescale if positive, returns if negative
	 */
	/*
	 * @ ensures timeScale = scale;
	 * @returns \nothing;
	 */
	public void manipulateTime(float scale)
	{
		if(scale<0){System.out.println("Invalid input, time scale cannot be negative");return;}
		this.timeScale = scale;
		this.setPaused(false);
		if(timeScale == 0) {
			this.setPaused(true);
		}
	}
	/**
	 * retrieveObject using the index of an object, find and return it
	 * @param index the index of the PhysicsObject
	 * @return the PhysicsObject at the index
	 */
	/*
	 * @requires this!=null;
	 * @requires scene!=null;
	 * @returns scene.objects.get(index);
	 */
	public PhysicsObject retrieveObject(int index)
	{
		if(index<0||index>scene.getObjectList().length-1){System.out.println("Invalid input, cannot retrieve from out of bounds index");return null;}
		return scene.getObject(index);
		
	}
	/**
	 * destroyObject removes a PhysicsObject from the system
	 * @param p the PhysicsObject that must be Destroyed
	 */
	/*
	 * @requires scene!=null;
	 * @requires this!=null;
	 * @requires scene.getObjects.get(p)==p;
	 * @ensures scene.getObjects.get(p)!=p;
	 * @returns \nothing;
	 */
	public void destroyObject(PhysicsObject p)
	{
		if(p==null){System.out.println("Illegal Input, cannot remove null object");return;}
		freeIndices.add(p.getIndex());
		scene.removeObject(p);
	}
	/*
	 * @ requires scene!=null;
	 * @requires this!=null;
	 * @returns \nothing;
	 */	//public void changeObject(){}//we should discuss if we need this and how to use it
	public void changeSceneProperties()
	{
		
	}
	/**
	 * saveScene saves the current scene into a SceneFile
	 */
	/*
	 * @requires scene!=null;
	 * @requires this!=null;
	 * @returns \nothing;
	 */
	public void saveScene(String filename)
	{
		sf.addToBuffer(scene.toString());
		sf.writeScene(filename);
	}
	/**
	 * loadScene loads a SceneFile into the system replacing the current scene with the loaded one
	 * @param s the SceneFile to load
	 */
	/*
	 * @requires s != null;
	 * @returns \nothing;
	 */
	public void loadScene(String filename)
	{
		if(filename==null){System.out.println("Illegal Input, cannot have null file name");return;}
		this.scene = sf.LoadScene(filename);
	}
	/**
	 * Detects whether or not Objects are in collision
	 */
	/*
	 * @ requires this!=null;
	 * @ requires scene!=null;
	 * @ requires scene.objects.size()>1;
	 * @ returns \nothing;
	 */
	public void detectCollisions()
	{
		if(scene == null){System.out.println("Scene is not set, cann		colliders.clear();ot detect collisions.");}
		
		// iterate over all the objects and check for collision
		for(PhysicsObject o : scene.getObjectList()){
			if(o!=null){
				o.temp_transform.clear();
				// grab all the quadnodes that this object overlaps or contains
				//LinkedList<QuadNode> intersect = scene.getQuadTree().findIntersect(o.getBoundingBox());
				
				// iterate through all the quadnodes that are intersecting the object 'o'
				//for(QuadNode node : intersect){
					// check for collision of each object in 'node' against 'o'
					for(PhysicsObject p : scene.getObjectList()){
						if(p != null)
							if(p!=o && !p.colliders.contains(o.getIndex())){
								Collision result = o.isColliding(p);
								
								if(result != null){
									// Collision occurred!
									this.collisions.add(result);
								}
							}
					}
				//}
			}
		}
	}
	/**
	 * relocateObjects checks the objects in the quadtree and places them in a new position in the tree if
	 * there exists a better place for them to be.
	 */
	/*
	 * @returns \nothing;
	 * @requires this!=null;
	 */
	public void relocateObjects(){
		// iterate over all the objects and check to see if there is
		// a more suitable place for them in the quad tree.
		for(PhysicsObject o : scene.getObjectList()){
			if(o != null){
				// if the object is no longer fully contained in its' parent, look for a better one.
				if(o.parent.getKey().Contains(o.getBoundingBox()) != IntersectRes.contains){
					// quad nodes that contain o
					LinkedList<QuadNode> intersection = scene.getQuadTree().findIntersect(o.getBoundingBox());
					
					// find the node in 'intersection' which is the deepest.
					QuadNode deepest = scene.getQuadTree().getRoot();
					for(QuadNode n : intersection){
						if(n.getDepth() > deepest.getDepth())
							deepest = n;
					}
					
					// pull object out of its' old parent.
					o.parent.getData().remove(o);
					// add to the new parent.
					deepest.getData().add(o);
				}
			}
		}
	}
	
	/**
	 * resolveCollision calculates new velocities for all objects in the collisions
	 */
	/*
	 * @ requires this!=null;
	 * @ requires scene!=null;
	 * @ requires detectCollisions == true;
	 * @ returns \nothing;
	 */
	public void resolveCollisions(float t)
	{
		if(collisions.size() > 0) {
			for(Collision c : collisions) {
				PhysicsObject po1 = c.getObj1();
				PhysicsObject po2 = c.getObj2();
				Vector impact = c.getImpact();
				
				//forces these objects will be exerting on one another.
				float po1f = po1.getLinForceDir(impact).magnitude();
				float po2f = po2.getLinForceDir(impact).magnitude();
				Vector po1Force = Vector.add(/*Vector.Zero*/po1.getRotForceDir(impact), po1.getLinForceDir(impact));
				Vector po2Force = Vector.add(/*Vector.Zero*/po2.getRotForceDir(impact), po2.getLinForceDir(impact));
				
				/*if(!po1Force.equals(Vector.Zero))
				po1Force.normalize();
				if(!po2Force.equals(Vector.Zero))
				po2Force.normalize();
				
				po1Force.scl(po1f);
				po2Force.scl(po2f);*/
				
				Vector po1EffForce = Vector.add(po1Force, Vector.add(Vector.mul(po1Force, new Vector(-2,-2)), po2Force));
				Vector po2EffForce = Vector.add(po2Force, Vector.add(Vector.mul(po2Force, new Vector(-2,-2)), po1Force));
				
				po1.applyForce(po1EffForce/*Vector.sub(po2Force,po1Force)*/, impact);
				po2.applyForce(po2EffForce/*Vector.sub(po1Force,po2Force)*/, impact);
			}
		}
	}
	
	/**
	 * setPaused used to pause and unpause the scene
	 * @param p if true, the scene will pause, if false the scene be not be paused
	 */
	/*
	 * @assignable paused;
	 * @ensures paused == p;
	 */
	public void setPaused(boolean p)
	{
		paused = p;
	}
	/**
	 * setCollisions used to turn collision detection on and off, when it is off, objects will not interact
	 * @param c whether or not to use collisions
	 */
	/*
	 * @assignable detectCollisions;
	 * @ensures detectCollisions == c;
	 */
	public void setCollisions(boolean c)
	{
		detectCollisions = c;
	}
	/**
	 * enableLog if the value true is passed, the system will write to a logfile after every update.  If the passed value is false, it will not, or stop writing
	 * @param l whether the program should be writing to the logfile
	 */
	/*
	 * @assignable writeToLog;
	 * @ensures writeToLog == l;
	 */
	public void enableLog(boolean l)
	{
		writeToLog = l;
	}
	/**
	 * get the current time scale of the system for debugging purposes
	 * @returns the time scale
	 */
	/*
	 * @returns this.timeScale;
	 */
	public float getTimeScale() {
		return timeScale;
	}
	/** determine if the simulation is paused or not for debugging purposes
	 * @returns paused
	 */
	/*
	 * @returns this.timeScale;
	 */
	public boolean getPaused() {
		return paused;
	}
	
}
